﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect.Nui;
using System.Reactive.Subjects;
using System.Reactive.Linq;

namespace Kinect.KinectMouseControl
{
    public class ClosestTrackedSkeletonFilter : IEventPublisher<SkeletonData>
    {
        IObservable<SkeletonData> IEventPublisher<SkeletonData>.EventObservable { get { return _skeletonDataSubject; } }
        private Subject<SkeletonData> _skeletonDataSubject;
        private int _skeletonPublishMilliseconds;
        private IEventPublisher<SkeletonFrame> _skeletonFramePublisher;

        public ClosestTrackedSkeletonFilter(IEventPublisher<SkeletonFrame> skeletonFramePublisher, int skeletonPublishMilliseconds)
        {
            _skeletonFramePublisher = skeletonFramePublisher;
            _skeletonPublishMilliseconds = skeletonPublishMilliseconds;
            SetupSkeletonDataObserver();
        }

        private void SetupSkeletonDataObserver()
        {
            _skeletonDataSubject = new Subject<SkeletonData>();
            _skeletonFramePublisher
                .EventObservable
                .Sample(TimeSpan.FromMilliseconds(_skeletonPublishMilliseconds))
                .Subscribe(FilterTrackedSkeletons);
        }

        private void FilterTrackedSkeletons(SkeletonFrame skeletonFrame)
        {
            var trackedSkeletons = skeletonFrame.Skeletons.Where(s => s.TrackingState == SkeletonTrackingState.Tracked);
            var orderedTrackedSkeletons = trackedSkeletons.OrderBy(sp => sp.Position.Z);
            var closestSkeleton = orderedTrackedSkeletons.FirstOrDefault();

            if (closestSkeleton != null)
            {
                _skeletonDataSubject.OnNext(closestSkeleton);
            }
        }
    }
}
